﻿#include "Spp.h"
#include <cmath>
#include <iostream>
#include <iomanip>

Spp::Spp(TimeSys* timeSys, ReadN* readN, ReadO* readO, GNSSconsts* gnssConsts)
    : mTimeSys(timeSys), mReadN(readN), mReadO(readO), mGnssConsts(gnssConsts) {

    // 初始化卫星位置计算器
    mBdsSatPos = new BDSSatPos(timeSys, readN);
    mGpsSatPos = new GPSSatPos(timeSys, readN);

    // 初始化误差改正模型
    mIonoCorr = new IonoCorr(gnssConsts, &readN->get_head());
    mTropCorr = new TropCorr(gnssConsts);

    // 初始化坐标系统
    BDSConsts bdsConsts;
    mCoordSys = new BDCS(bdsConsts.get_refEllip());
}

Spp::~Spp() {
    delete mBdsSatPos;
    delete mGpsSatPos;
    delete mIonoCorr;
    delete mTropCorr;
    delete mCoordSys;
}

SppResult Spp::calculate(const obsEpoch& epoch) {
    SppResult result;
    result.numSat = 0;
    result.clockBias = 0.0;
    result.pdop = 999.9;
    result.sigma = 0.0;

    // 1. 获取观测时间和卫星数据
    BaseT obsTime = mTimeSys->str2baseT(epoch.time);
    CalendT calendTime = mTimeSys->str2calendT(epoch.time);
    SOW sowTime = mTimeSys->calend2sow(calendTime);

    // 2. 处理卫星数据，获取卫星位置和伪距观测值
    vector<XYZ> satPositions;
    vector<double> pseudoRanges;
    vector<string> prns;

    processSatellites(epoch, satPositions, pseudoRanges, prns);
    if (satPositions.size() < 4) {
        cerr << "Error: Not enough satellites (" << satPositions.size() << ") for SPP." << endl;
        return result;
    }

    // 3. 初始近似位置 (可以设为0或上次定位结果)
    XYZ approxPos = { 0, 0, 0 };
    XYZ prevPos = approxPos;
    double clockBias = 0.0;

    // 4. 迭代最小二乘解算
    Matrix designMatrix;
    vector<double> obsVector;
    vector<double> ranges;
    Matrix Qxx;  // 协方差矩阵

    const int maxIter = 10;
    const double convThreshold = 0.1;  // 收敛阈值 (米)

    for (int iter = 0; iter < maxIter; ++iter) {
        // 构建设计矩阵和观测向量
        buildDesignMatrix(approxPos, satPositions, pseudoRanges, designMatrix, obsVector, ranges);

        // 最小二乘解算
        Matrix A = designMatrix;
        Matrix AT = A.transpose();
        Matrix ATA = AT * A;

        if (!ATA.invert()) {
            cerr << "Error: Matrix inversion failed." << endl;
            return result;
        }

        // 创建观测值矩阵
        Matrix obsMatrix(obsVector.size(), 1);
        for (size_t i = 0; i < obsVector.size(); ++i) {
            obsMatrix[i][0] = obsVector[i];
        }
        Matrix ATL = AT * obsMatrix;
        Matrix dx = ATA * ATL;

        // 更新位置和钟差
        approxPos.x += dx[0][0];
        approxPos.y += dx[1][0];
        approxPos.z += dx[2][0];
        clockBias += dx[3][0];

        // 计算DOP值
        calculateDOP(designMatrix, result.pdop);

        // 检查收敛
        double delta = sqrt(pow(approxPos.x - prevPos.x, 2) +
            pow(approxPos.y - prevPos.y, 2) +
            pow(approxPos.z - prevPos.z, 2));

        if (delta < convThreshold) {
            break;
        }

        prevPos = approxPos;
    }

    // 5. 计算残差和标准差
    vector<double> residuals;
    for (size_t i = 0; i < satPositions.size(); ++i) {
        double r = approxRange(satPositions[i], approxPos) + clockBias;
        residuals.push_back(pseudoRanges[i] - r);
    }

    double sumSq = 0.0;
    for (double r : residuals) {
        sumSq += r * r;
    }
    result.sigma = sqrt(sumSq / (satPositions.size() - 4));

    // 6. 填充结果
    result.position = approxPos;
    result.blh = mCoordSys->XYZ2BLH(approxPos);
    result.clockBias = clockBias;
    result.numSat = static_cast<int>(satPositions.size());

    return result;
}

void Spp::processSatellites(const obsEpoch& epoch,
    vector<XYZ>& satPositions,
    vector<double>& pseudoRanges,
    vector<string>& prns) {
    BaseT obsTime = mTimeSys->str2baseT(epoch.time);
    CalendT calendTime = mTimeSys->str2calendT(epoch.time);
    SOW sowTime = mTimeSys->calend2sow(calendTime);

    for (const auto& sat : epoch.listObsSat) {
        string prn = sat.satID;
        double pr = sat.values[0];  // 第一个观测值是伪距

        // 获取卫星位置
        XYZ satPos;
        if (prn[0] == 'C') {  // 北斗卫星
            satPos = mBdsSatPos->get_satPos(prn, obsTime);
        }
        else if (prn[0] == 'G') {  // GPS卫星
            satPos = mGpsSatPos->get_satPos(prn, obsTime);
        }
        else {
            continue;  // 忽略其他系统
        }

        // 忽略无效位置
        if (satPos.x == 0 && satPos.y == 0 && satPos.z == 0) {
            continue;
        }

        // 计算高度角，忽略低高度角卫星
        XYZ approxPos = { 0, 0, 0 };  // 初始近似位置
        BLH blh = mCoordSys->XYZ2BLH(approxPos);
        Azel azel = mCoordSys->get_azel(approxPos, satPos);

        if (azel.ele < 15.0 * D2R) {  // 高度角小于15度
            continue;
        }

        // 应用误差改正
        double ionoCorr = mIonoCorr->get_ionoCorr(blh, azel, sowTime);
        double tropCorr = mTropCorr->get_tropCorr(blh, azel.ele);

        // 改正后的伪距
        double correctedPr = pr - ionoCorr - tropCorr;

        // 保存数据
        satPositions.push_back(satPos);
        pseudoRanges.push_back(correctedPr);
        prns.push_back(prn);
    }
}

void Spp::buildDesignMatrix(const XYZ& approxPos,
    const vector<XYZ>& satPositions,
    const vector<double>& pseudoRanges, // 新增参数
    Matrix& designMatrix,
    vector<double>& obsVector,
    vector<double>& ranges)
{
    int n = static_cast<int>(satPositions.size());
    designMatrix = Matrix(n, 4);
    obsVector.clear();
    ranges.clear();

    for (int i = 0; i < n; ++i) {
        const XYZ& satPos = satPositions[i];
        double range = approxRange(satPos, approxPos);
        ranges.push_back(range);

        // 构建设计矩阵
        designMatrix[i][0] = (approxPos.x - satPos.x) / range;
        designMatrix[i][1] = (approxPos.y - satPos.y) / range;
        designMatrix[i][2] = (approxPos.z - satPos.z) / range;
        designMatrix[i][3] = 1.0;  // 钟差系数

        // 构建观测向量：观测值 - 计算值
        obsVector.push_back(pseudoRanges[i] - range); // 现在可以使用pseudoRanges
    }
}
double Spp::approxRange(const XYZ& satPos, const XYZ& recPos) {
    double dx = recPos.x - satPos.x;
    double dy = recPos.y - satPos.y;
    double dz = recPos.z - satPos.z;
    return sqrt(dx * dx + dy * dy + dz * dz);
}

void Spp::calculateDOP(const Matrix& designMatrix, double& pdop) {
    Matrix A = designMatrix;
    Matrix AT = A.transpose();
    Matrix ATA = AT * A;

    if (!ATA.invert()) {
        pdop = 999.9;
        return;
    }

    // PDOP = sqrt(Qxx + Qyy + Qzz)
    pdop = sqrt(ATA[0][0] + ATA[1][1] + ATA[2][2]);
}